﻿#include "Matrix.h"
#include "Constant.h"
#include "CoordSys.h"
#include "TimeSys.h"
#include "ReadN.h"
#include "ReadO.h"
#include "GNSSSatPos.h"
#include "IonoCrr.h"
#include "TropCrr.h"
#include "SPP.h"
#include <iostream>
#include <iomanip>
#include <vector>
#include <cstdlib> /*用于使用语句 system("pause");*/
using namespace std;

/* 用于测试类 */
int main() {
    /* 测试常数类 */
    GNSSconsts* pGnssConst;
    BDSConsts bdsConst;
    pGnssConst = &bdsConst; /* 用法：基类指针指向派生类实例化对象 */
    cout << fixed << pGnssConst->get_refEllip().f << endl;

    /* 测试矩阵类 */
    cout << "/* 测试矩阵类 */" << endl;
    Matrix matTest;
    matTest.test();

    /* 测试坐标系统类 */
    cout << "/* 测试坐标系统类 */" << endl;
    CoordSys* pCoordSys;
    BDCS bdcs(bdsConst.get_refEllip());
    pCoordSys = &bdcs;
    pCoordSys->test();

    /* 测试时间系统类 */
    cout << "/* 测试时间系统类 */" << endl;
    CalendT t = { 2016,7,14,1,0,0 };/* N 文件中第一个星历的时间，对应BDS 549周， 349200秒 */
    BDSTime mBDST;
    TimeSys* pTimeSys = &mBDST;
    pTimeSys->print_time(t);
    cout << " 对应的BDS时间为：" << endl;
    SOW t2 = pTimeSys->calend2sow(t);
    pTimeSys->print_time(t2);

    /* 测试 Read N 文件类 */
    ReadN mReadN("1234560660442.17N");
    if (!mReadN.parseFile()) return false;
    if (!mReadN.print_eph()) return false;

    /* 测试 Read O 文件类 */
    string filename = "1234560660442.17O";// 声明要读取的文件名   
    ReadO mReadO(filename);  // 创建 ReadO 对象
    if (!mReadO.parse_file()) return false;
    mReadO.print_head();// 显示文件头部信息
    mReadO.print_body(2);// 显示前 10 个历元的观测数据
    vector<obsEpoch>* mObs = mReadO.get_obs();
    BaseT tr;
    tr = pTimeSys->str2baseT(mObs->at(0).time); // 获取O文件中第一次观测的的时间

    /* 测试卫星位置计算类 */
    cout << "/* 卫星位置计算类 */" << endl;
    GNSSSatPos* pSatPos;
    BDSSatPos mBDSSatPos(pTimeSys, &mReadN);
    pSatPos = &mBDSSatPos; /* 用法：基类指针指向派生类实例化对象 */
    XYZ ps1;
    ps1 = pSatPos->get_satPos("C01", tr);
    printf("C01 卫星位置：X=%.3f m, Y=%.3f m, Z=%.3f m\n\n", ps1.x, ps1.y, ps1.z);

    /* 电离层延迟误差改正 */
    cout << "/*测试电离层、对流层延迟误差改正类 */" << endl;
    IonoCrr mIonCor(pGnssConst, &mReadN.get_head());
    CalendT tObs = { 2017, 3, 7, 5, 6, 35,0.00 };
    SOW sow = pTimeSys->calend2sow(tObs);

    XYZ rr = { -2091342.6543,4800418.4462,3629758.3665 }; /* 接收机坐标概况值 */
    BLH blh = pCoordSys->XYZ2BLH(rr);         /* 接收机大地坐标 */
    Azel azel = pCoordSys->get_azel(rr, ps1); /* 计算接收机到卫星的方位角和高度角 */
    double ionCor = mIonCor.get_ionoCorr(blh, azel, sow);
    cout << "电离层延迟误差：" << fixed << ionCor << endl ;
    /* 对流层延迟误差改正 */
    TropCrr mTropCrr(pGnssConst);
    double tropCrr = mTropCrr.get_tropCorr(blh, azel.ele);
    cout << "对流层延迟误差：" << fixed << tropCrr << endl << endl;

    // 单点定位计算
    SPPSolver singleSPP(&bdcs, &mBDSSatPos, &mIonCor, &mTropCrr, &mReadO, &mReadN, pTimeSys);

    // 设置初始位置
    XYZ rcvPos = { 0, 0, 0 };
    double clockErr = 0;

    // 处理第一个历元
    if (singleSPP.solve_epoch(0, rcvPos, clockErr)) {
        cout << "单点定位结果: \n"
            << "X: " << rcvPos.x << " m\n"
            << "Y: " << rcvPos.y << " m\n"
            << "Z: " << rcvPos.z << " m\n";
    }
    else {
        cerr << "定位失败" << endl;
    }
    return 0;
}



/*
GNSS单点定位全流程解析
一、系统初始化阶段
    1. 基础组件创建与初始化
        ○ 创建并初始化时间系统对象（BDSTime）：处理GNSS时间标准（如GPS周 / 秒、北斗时等），确保时间同步。
        ○ 创建并初始化坐标系统对象（BDCS）：定义定位结果输出的坐标系（如ECEF、WGS84等）。
        ○ 创建观测文件读取器（ReadO）和导航文件读取器（ReadN）：分别用于解析观测数据（O文件）和卫星星历数据（N文件）。
        ○ 创建卫星位置计算器（BDSSatPos）：基于广播星历计算卫星实时位置。
        ○ 创建误差改正模型（IonoCrr和TropCrr）：用于电离层延迟和对流层延迟修正。
二、数据准备阶段
    1. 解析导航文件（N文件）: 通过ReadN读取卫星星历数据（如轨道参数、钟差修正项等），获取可见卫星的广播星历信息。
    2. 解析观测文件（O文件）:通过ReadO提取伪距观测值（含卫星ID、伪距测量、观测时间等）。
    3. 数据有效性验证:检查文件解析是否完整，确认观测数据与星历数据的时效匹配性，排除无效数据。
三、构建单点定位处理器
    创建SPPSolver核心处理模块，集成所有初始化组件（坐标系统、卫星位置计算、误差修正模型、数据读取器等），形成完整定位解算框架。
四、历元级定位迭代解算（逐历元处理）
核心流程：
    1. 数据提取:从观测文件中获取当前历元的所有有效观测值（卫星ID + 伪距）。
    2. 初始位置设定
        ○ 首次定位：使用预设零值或概略坐标（如用户输入的初始位置）作为迭代起点。
        ○ 后续历元：优先采用上一历元定位结果作为初始值，加速收敛。
    3. 迭代计算（最多MAX_ITERATIONS次）
        ○ Step 1：卫星位置与钟差计算:根据广播星历，计算每颗卫星在当前历元的ECEF坐标及卫星钟差。
        ○ Step 2：几何距离估算:基于卫星位置与接收机初始位置，计算理论几何距离（不考虑误差）。
        ○ Step 3：误差修正:通过电离层模型（IonoCrr）和对流层模型（TropCrr）计算延迟修正量，并应用于伪距观测值。
        ○ Step 4：构建观测方程
            ■ 计算方向余弦矩阵（设计矩阵）：描述卫星与接收机的几何关系。
            ■ 构建伪距残差观测向量（实际伪距 - 修正后的理论距离）。
        ○ Step 5：最小二乘解算:通过加权最小二乘法（LS）求解位置增量（ΔX, ΔY, ΔZ）和接收机钟差（Δt）。
        ○ Step 6：收敛判断:若位置增量小于预设阈值（如0.1米）或达到最大迭代次数，终止迭代；否则更新位置并重新计算。
    4. 定位结果输出
        ○ 最终定位坐标：输出ECEF坐标系下的三维坐标（X, Y, Z）。
        ○ 坐标转换：按需转换为大地坐标系（BLH：纬度、经度、高程）。
        ○ 钟差输出：以距离单位（如米）表示接收机钟差估计值。
*/

